#include "imu_factor.h"

ImuFactor::ImuFactor(std::shared_ptr<ImuPreIntegration> imu_preintegration) : imu_preintegration_(imu_preintegration) {

}

void ImuFactor::ComputeResidualAndJacobianPoseGraph(const State& last_state, const State& state, Eigen::Matrix<double, 5, 1> &residual, Eigen::Matrix<double, 5, 8> &jacobianXi, Eigen::Matrix<double, 5, 5> &jacobianXj) {
	double dt = imu_preintegration_->dt_;

	double bg = last_state.bg;
    Eigen::Vector2d ba = last_state.ba;

	SO2 dR = imu_preintegration_->GetDeltaRotation(bg);
	Eigen::Vector2d dv = imu_preintegration_->GetDeltaVelocity(bg, ba);
	Eigen::Vector2d dp = imu_preintegration_->GetDeltaPosition(bg, ba);

	Eigen::Matrix2d Rit = SO2(last_state.R).inverse(); 

	//（九）(30)
	residual(0,0) = SO2::exp(dR.inverse() * SO2(last_state.R).inverse() * SO2(state.R).matrix());

	//（九）(31)
	residual.block<2,1>(1,0) = Rit * (state.v - last_state.v) - dv;

	//（九）(32)
	residual.block<2,1>(3,0) = Rit * (state.p - last_state.p - last_state.v * dt) - dp;

	cout << "ImuFactor residual =========== : " << residual.transpose() << endl;
	
	// compute jacobian
	jacobianXi.setZero();
	//   col=0   col=1,2 col=3,4 col=5   col=6,7 col=8   col=9,10 col=11,12
    // | dR1/dR1 dR1/dv1 dR1/dp1 dR1/dbg dR1/dba dR1/dR2 dR1/dv2 dR1/dp2|
    // | dv1/dR1 dv1/dv1 dv1/dp1 dv1/dbg dv1/dba dv1/dR2 dv1/dv2 dv1/dp2|
    // | dp1/dR1 dp1/dv1 dp1/dp1 dp1/dbg dp1/dba dp1/dR2 dp1/dv2 dp1/dp2|
    // | dR2/dR1 dR2/dv1 dR2/dp1 dR2/dbg dR2/dba dR2/dR2 dR2/dv2 dR2/dp2|
    // | dv2/dR1 dv2/dv1 dv2/dp1 dv2/dbg dv2/dba dv2/dR2 dv2/dv2 dv2/dp2|
    // | dp2/dR1 dp2/dv1 dp2/dp1 dp2/dbg dp2/dba dp2/dR2 dp2/dv2 dp2/dp2|
	// compute jacobian, last_state
	// dR/dR1
	jacobianXi(0,0) = -1; // (33)
	// dR/dbg1
	jacobianXi(0,5) = -dt; // (35)

	// dv/dR1
	jacobianXi.block<2,1>(1,0) = -SO2::so2(1) * Rit * (state.v - last_state.v); // (38)
	// dv/dv1
	jacobianXi.block<2,2>(1,3) = -Rit; // (36)
	// dv/dbg1
	jacobianXi.block<2,1>(1,5) = -imu_preintegration_->dV_dbg_;	// -(27)
	// dv/dba1
	jacobianXi.block<2,2>(1,6) = -imu_preintegration_->dV_dba_; // -(26)

	// dp/dR1
	jacobianXi.block<2,1>(3,0) = -SO2::so2(1) * Rit * (state.p - last_state.p - last_state.v * dt); // (42)
	// dp/dp1
	jacobianXi.block<2,2>(3,1) = -Rit; // (39)
	// dp/dv1
	jacobianXi.block<2,2>(3,3) = -Rit * dt; // (41)
	// dp/dbg1
	jacobianXi.block<2,1>(3,5) = -imu_preintegration_->dP_dbg_;
	// dp/dba1
	jacobianXi.block<2,2>(3,6) = -imu_preintegration_->dP_dba_;

	// compute jacobian, state
	jacobianXj.setZero();

	// dR/dR2
	jacobianXj(0,0) = 1; // (34)
	// dv/dv2
	jacobianXj.block<2,2>(1,3) = Rit; // (37)
	// dp/dv2
	jacobianXj.block<2,2>(3,1) = Rit; // (40)
}